#include "bonderworkholder.h"

void BonderWorkHolder::getNewTray()
{
    checkIsInit();

    if (!hasSynchronizeHasTray)
    {
        synchronizeHasTray();
        hasSynchronizeHasTray = true;
    }
    if (!whState->bonderWHHasTray())
    {
        if (isDispWHStopped)
        {
            handleStop();
            return;
        }
        getNewTrayImpl(false, true);
    }
}

void BonderWorkHolder::setSensorTrayMap(QJsonValue data)
{
    sensorTrayMap->fromJson(data);
}

void BonderWorkHolder::pushOutTray()
{
    if (hasMoveBeltToCheckHasTrayAtExitOfSTC)
    {
        stcWorker->makeSureHasNotTrayAtTheExit(false);
    }
    else
    {
        stcWorker->makeSureHasNotTrayAtTheExit(true);
        hasMoveBeltToCheckHasTrayAtExitOfSTC = true;
    }

    pushOutTrayImpl();
}

void BonderWorkHolder::getSensor()
{
    if (!whState->bonderWHHasTray())
    {
        pushEvent(WorkerEvent("getNewTray"));
        pushEvent(WorkerEvent("getSensor"));
        return;
    }

    for (int columnIndex = currentColumnIndex; columnIndex >= 0; columnIndex--)
    {
        currentColumnIndex = columnIndex;
        for (int rowIndex = 0; rowIndex < sensorTrayConfig->rows(); rowIndex++)
        {
            auto sensorData = sensorTrayMap->getData<SensorTrayMaterialData>(rowIndex, columnIndex);
            if (sensorData->materialState() == SensorTrayMaterialData::BondPrOk)
            {
                m_gotSensor = true;
                return;
            }
            if (sensorData->materialState() == SensorTrayMaterialData::DispCheckOk)
            {
                moveToPrPosImpl(rowIndex, columnIndex);
                PrOffset prOffset;
                if (stSensorLocation->performPR(prOffset))
                {
                    lastSensorOffset = prOffset;
                    lastSensorIndex.setX(rowIndex);
                    lastSensorIndex.setY(columnIndex);
                    sensorData->setMaterialState(SensorTrayMaterialData::BondPrOk);
                    m_gotSensor = true;
                    bwhOpticZ->moveTo(SAxisPosName::PostBondPrPos, false);
                    return;
                }
                else
                {
                    sensorData->setMaterialState(SensorTrayMaterialData::BondPrNg);
                    continue;
                }
            }
        }
    }

    // have to change tray first
    pushEvent(WorkerEvent("pushOutTray"));
    pushEvent(WorkerEvent("getNewTray"));
    pushEvent(WorkerEvent("getSensor"));
}

void BonderWorkHolder::upOpticZ()
{
    auto postBondPrPos = bwhOpticZ->getPos<SingleAxisPos>(SAxisPosName::PostBondPrPos);
    bwhOpticZ->axis()->absForceMove(postBondPrPos->pos());
    bwhOpticZ->axis()->waitArrivedPos(postBondPrPos->pos(), postBondPrPos->precision());
}

void BonderWorkHolder::postBond(int lensRowIndex, int lensColumnIndex)
{
    auto data = sensorTrayMap->getData<SensorTrayMaterialData>(lastSensorIndex.x(), lastSensorIndex.y());
    if (data->materialState() != SensorTrayMaterialData::Cured)
    {
        throw SilicolAbort(
            tr("Logic error! Try to post bond but sensor is not in Cured state! RowIndex: %1, ColumnIndex: %2")
                .arg(lastSensorIndex.x())
                .arg(lastSensorIndex.y()),
            EX_LOCATION);
    }

    QImage image = postBondLocation->getImage();

    QtConcurrent::run([image, data, lensRowIndex, lensColumnIndex, this] {
        PrOffset postBondOffset;
        if (!postBondLocation->performPR(image, postBondOffset, true))
        {
            data->setMaterialState(SensorTrayMaterialData::PostBondPrNg);
            savePostBondData(lastSensorIndex, QPoint(lensRowIndex, lensColumnIndex), lastSensorOffset, postBondOffset,
                             false, false);
            return;
        }
        PrOffset shift = (postBondOffset - lastSensorOffset) * 1000;
        correctPostBondShift(shift);
        showPostBondShift(shift);
        bool postBondResult
            = qAbs(shift.X) < qAbs(bwhConfig->postBondLimitX()) && qAbs(shift.Y) < qAbs(bwhConfig->postBondLimitY());
        if (postBondResult)
        {
            data->setMaterialState(SensorTrayMaterialData::PostBondOk);
        }
        else
        {
            data->setMaterialState(SensorTrayMaterialData::PostBondNg);
        }
        savePostBondData(lastSensorIndex, QPoint(lensRowIndex, lensColumnIndex), lastSensorOffset, postBondOffset, true,
                         postBondResult);
    });
}

void BonderWorkHolder::dispWHStopped()
{
    isDispWHStopped = true;
}

void BonderWorkHolder::startLoadUnloadTrayLoopTest(int times)
{
    checkIsInit();

    if (isLoadUnloadTrayLoopTesting)
    {
        return;
    }

    isLoadUnloadTrayLoopTesting = true;
    AutoResetBool arb(&isLoadUnloadTrayLoopTesting, false);
    for (int i = 0; i < times; i++)
    {
        if (!isLoadUnloadTrayLoopTesting)
        {
            return;
        }
        manuallyGetNewTray(false, false);
        if (!isLoadUnloadTrayLoopTesting)
        {
            return;
        }
        manuallyPushOutTray();
    }
}

void BonderWorkHolder::stopLoadUnloadTrayLoopTest()
{
    isLoadUnloadTrayLoopTesting = false;

    WorkerEvent abortEvent(SMD::Abort);
    pushEvent(abortEvent);
    emit postEvent(WorkerName::STIE, abortEvent);
    emit postEvent(WorkerName::STOE, abortEvent);
    emit postEvent(WorkerName::DispenseWorkHolder, abortEvent);
}

void BonderWorkHolder::resetLoadUnloadTrayLoopTest()
{
    WorkerEvent resetEvent(SMD::Reset);
    pushEvent(resetEvent);
    emit postEvent(WorkerName::STIE, resetEvent);
    emit postEvent(WorkerName::STOE, resetEvent);
    emit postEvent(WorkerName::DispenseWorkHolder, resetEvent);
}

void BonderWorkHolder::manuallyGetNewTray(bool needShotGlue, bool showTip)
{
    checkIsInit();

    hasCancelManuallyGetNewTray = false;

    if (detectReallyHasTray())
    {
        if (showTip)
        {
            UIOperation::getIns()->showTip(tr("Bonder work holder already detected Tray!"));
        }
        return;
    }

    QVariantList args;
    args << false;
    WorkerEvent getNewTrayEvent("manuallyGetNewTray", args);
    emit sendEvent(WorkerName::DispenseWorkHolder, getNewTrayEvent);

    if (hasCancelManuallyGetNewTray)
    {
        return;
    }

    if (needShotGlue && !whState->dispWHTrayHasDoneWork())
    {
        WorkerEvent performAllSensorShotGlueEvent("performAllSensorShotGlue");
        emit postEvent(WorkerName::RightDispenseHead, performAllSensorShotGlueEvent);
    }

    if (hasCancelManuallyGetNewTray)
    {
        return;
    }
    getNewTrayImpl(true, needShotGlue);
}

void BonderWorkHolder::manuallyPushOutTray()
{
    checkIsInit();

    if (!detectReallyHasTray())
    {
        UIOperation::getIns()->showTip(tr("Bonder work holder did not detect Tray!"));
        return;
    }
    stcWorker->makeSureHasNotTrayAtTheExit(true);

    pushOutTrayImpl();
}

void BonderWorkHolder::cancelManuallyGetNewTray()
{
    hasCancelManuallyGetNewTray = true;
}

void BonderWorkHolder::moveToPrPos(int unitRowIndex, int unitColumnIndex, int rowIndex, int columnIndex)
{
    checkIsInit();
    sensorTrayConfig->getValidUiIndex(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);
    moveToPrPosImpl(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);
}

void BonderWorkHolder::moveToPerformSensorPr(int unitRowIndex, int unitColumnIndex, int rowIndex, int columnIndex)
{
    checkIsInit();
    sensorTrayConfig->getValidUiIndex(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);
    moveToPrPosImpl(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);
    getSensorPrOffset();
}

void BonderWorkHolder::moveToPerformPostBondPr(int unitRowIndex, int unitColumnIndex, int rowIndex, int columnIndex)
{
    checkIsInit();
    sensorTrayConfig->getValidUiIndex(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);
    moveToPrPosImpl(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);
    getPostBondOffset();
}

void BonderWorkHolder::clearData()
{
    postBondDataFileName.clear();
}

void BonderWorkHolder::downBeltZAndTrackZ()
{
    bonderWHBeltZ->moveTo(SAxisPosName::DownPos);
    bonderWHTrackZ->moveTo(SAxisPosName::DownPos);
}

void BonderWorkHolder::upBeltZ(bool moveTrackX)
{
    if (moveTrackX)
    {
        bonderWHTrackZ->moveTo(SAxisPosName::UpPos);
        bonderWHTrackX->moveTo(SAxisPosName::LoadTrayPos);
        bonderWHVacuum->set(false);
        bonderWHBeltZ->moveTo(SAxisPosName::UpPos);
    }
    else
    {
        bonderWHVacuum->set(false);
        bonderWHTrackZ->moveTo(SAxisPosName::UpPos, false);
        bonderWHBeltZ->tryToMove(SAxisPosName::UpPos);
        bonderWHTrackZ->waitArrivedPos(SAxisPosName::UpPos);
    }
}

void BonderWorkHolder::downBeltZ()
{
    bonderWHBeltZ->moveTo(SAxisPosName::DownPos);
    bonderWHTrackZ->moveTo(SAxisPosName::DownPos);
    bonderWHVacuum->set(true);
}

void BonderWorkHolder::synchronizeHasTray()
{
    bool reallyHasTray = detectReallyHasTray();
    if (reallyHasTray)
    {
        upBeltZ(false);
        downBeltZ();

        for (int i = 0; i < sensorTrayConfig->rows(); i++)
        {
            for (int j = 0; j < sensorTrayConfig->columns(); j++)
            {
                auto data = sensorTrayMap->getData<SensorTrayMaterialData>(i, j);
                if (data->materialState() == SensorTrayMaterialData::BondPrOk)
                {
                    data->setMaterialState(SensorTrayMaterialData::DispCheckOk);
                }
            }
        }
    }

    if (reallyHasTray == whState->bonderWHHasTray())
    {
        return;
    }
    else
    {
        qCWarning(logCate()) << tr("Software remembered has tray: %1, Really has tray: %2. Now sycnchronize "
                                   "HasTrayState!")
                                    .arg(whState->bonderWHHasTray())
                                    .arg(reallyHasTray);

        whState->setBonderWHHasTray(reallyHasTray);
    }
}

bool BonderWorkHolder::detectReallyHasTray()
{
    //    upBeltZ(true);
    //    if (bonderWHStageTraySensor->get(-1))
    //    {
    //        return true;
    //    }
    //    bonderWHBelt->velocityMove(beltRunDir);
    //    QElapsedTimer timer;
    //    timer.start();
    //    while (timer.elapsed() < bwhConfig->detectHasTrayMaxTime())
    //    {
    //        if (bonderWHStageTraySensor->get(-1))
    //        {
    //            break;
    //        }
    //        QThread::msleep(10);
    //    }
    //    bonderWHBelt->stop();
    return bonderWHStageTraySensor->get(-1);
}

void BonderWorkHolder::getNewTrayImpl(bool isManuallyGetTray, bool needLastStationDoneWork)
{
    bonderWHStopperCylRight->set(true, false);
    upBeltZ(true);

    QElapsedTimer timer;
    timer.start();
    while (true)
    {
        if (timer.elapsed() > 20000)
        {
            qCInfo(logCate()) << tr("Waiting dispense work holder doing work...");
            timer.restart();
        }
        if (hasAbortEvent())
        {
            return;
        }
        if (isDispWHStopped)
        {
            handleStop();
            return;
        }
        if (isManuallyGetTray && hasCancelManuallyGetNewTray)
        {
            return;
        }
        if (whState->dispWHHasTray())
        {
            if (needLastStationDoneWork)
            {
                if (whState->dispWHTrayHasDoneWork())
                {
                    break;
                }
            }
            else
            {
                break;
            }
        }
        QThread::msleep(30);
    }

    bonderWHStopperCylLeft->set(false);
    bonderWHBelt->velocityMove(beltRunDir);
    if (isManuallyGetTray)
    {
        emit postEvent(WorkerName::DispenseWorkHolder, WorkerEvent("manuallyPushOutTray"));
    }
    else
    {
        emit postEvent(WorkerName::DispenseWorkHolder, WorkerEvent("pushOutTray"));
    }
    ErrorHandler::tryToHandleGeneralError<void>([this] {
        QElapsedTimer timer;
        timer.start();
        while (true)
        {
            if (timer.elapsed() > bwhConfig->getNewTrayTimeout())
            {
                bonderWHBelt->stop();
                throw GeneralError(workerName(), tr("Get new tray timeout!"));
            }
            if (bonderWHStageTraySensor->get(-1))
            {
                bonderWHBelt->stop();
                break;
            }
            QThread::msleep(10);
        }
    });
    resetRowColumnIndex();
    whState->setBonderWHHasTray(true);
    bonderWHStopperCylLeft->set(true, false);

    timer.restart();
    while (true)
    {
        if (stcWorker->beltZCanMove())
        {
            break;
        }
        if (hasAbortEvent())
        {
            return;
        }
        if (timer.elapsed() > 5000)
        {
            qCWarning(logCate()) << "Waiting STC push out tray...";
            timer.restart();
        }
        QThread::msleep(10);
    }
    downBeltZ();
    bonderWHStopperCylRight->set(false, false);

    bonderWHStopperCylLeft->wait(true);
}

void BonderWorkHolder::pushOutTrayImpl()
{
    if (!bonderWHTrackX->greaterThanPos(SAxisPosName::UnloadTrayPos))
    {
        bonderWHTrackX->moveTo(SAxisPosName::UnloadTrayPos);
    }
    bonderWHStopperCylRight->set(false, false);
    upBeltZ(false);
    bonderWHStopperCylRight->wait(false);

    stcWorker->pushEvent(WorkerEvent("pushOutTray"));
    bonderWHTrackX->moveTo(SAxisPosName::LoadTrayPos);

    whState->setBonderWHHasTray(false);
    sensorTrayMap->reset();
}

void BonderWorkHolder::moveToPrPosImpl(int unitRowIndex, int unitColumnIndex, int rowIndex, int columnIndex)
{
    bwhOpticZ->moveTo(SAxisPosName::BondPrPos, false);
    bwhOpticXY->moveTo(getPrPos(unitRowIndex, unitColumnIndex, rowIndex, columnIndex));
    bwhOpticZ->waitArrivedPos(SAxisPosName::BondPrPos);
}

void BonderWorkHolder::moveToPrPosImpl(int rowIndex, int columnIndex)
{
    int unitRowIndex = rowIndex / sensorTrayConfig->rowsInsideUnit();
    int rowIndexInsideUnit = rowIndex % sensorTrayConfig->rowsInsideUnit();
    int unitColumnIndex = columnIndex / sensorTrayConfig->columnsInsideUnit();
    int columnIndexInsideUnit = columnIndex % sensorTrayConfig->columnsInsideUnit();
    moveToPrPosImpl(unitRowIndex, unitColumnIndex, rowIndexInsideUnit, columnIndexInsideUnit);
}

QPointF BonderWorkHolder::getPrPos(int unitRowIndex, int unitColumnIndex, int rowIndex, int columnIndex)
{
    auto prLTPos = bwhOpticXY->getPos<XYModulePos>(XYPosName::PrLeftTop);
    double xDelta
        = unitColumnIndex * sensorTrayConfig->unitColumnDelta() + columnIndex * sensorTrayConfig->columnDelta();
    double yDelta = unitRowIndex * sensorTrayConfig->unitRowDelta() + rowIndex * sensorTrayConfig->rowDelta();
    return QPointF(prLTPos->xPos() - xDelta, prLTPos->yPos() - yDelta);
}

void BonderWorkHolder::resetRowColumnIndex()
{
    currentColumnIndex = sensorTrayConfig->columns() - 1;
}

void BonderWorkHolder::handleStop()
{
    clearEvent();
    stopEventHandled();
    sendEvent(WorkerName::BonderHead, WorkerEvent("bwhStopped"));
}

void BonderWorkHolder::savePostBondData(QPoint sensorIndex,
                                        QPoint lensIndex,
                                        PrOffset &sensorOffset,
                                        PrOffset &postBondOffset,
                                        bool isValidPostBondOffset,
                                        bool result)
{
    const QString dir = "./postBondData";
    const QString title
        = "sensorRowIndex,sensorColumnIndex,lensRowIndex,lensColumnIndex,sensorOffsetX,sensorOffsetY,sensorOffsetTheta,"
          "postBondOffsetX,postBondOffsetY,postBondOffsetTheta,postBondShiftX_um,postBondShiftY_um,postBondShiftTheta_"
          "deg,result\r\n";

    if (!QDir(dir).exists())
    {
        QDir().mkpath(QDir(dir).absolutePath());
    }
    if (postBondDataFileName.isEmpty())
    {
        postBondDataFileName = QDateTime::currentDateTime().toString("yyyy_MM_dd_hh_mm_ss_zzz") + ".csv";
    }

    QVariantList datas;
    datas << sensorIndex.x() << sensorIndex.y() << lensIndex.x() << lensIndex.y() << sensorOffset.X << sensorOffset.Y
          << sensorOffset.Theta;
    if (isValidPostBondOffset)
    {
        PrOffset shift = (postBondOffset - sensorOffset) * 1000;
        datas << postBondOffset.X << postBondOffset.Y << postBondOffset.Theta << shift.X << shift.Y << shift.Theta;
    }
    else
    {
        datas << "/"
              << "/"
              << "/"
              << "/"
              << "/"
              << "/";
    }
    datas << (result ? "Pass" : "Fail");

    auto line = (combineVariant(",", datas) + "\r\n").toUtf8();

    QFile f(QDir(dir).absoluteFilePath(postBondDataFileName));
    if (f.exists())
    {
        f.open(QIODevice::Append);
        f.write(line);
    }
    else
    {
        f.open(QIODevice::WriteOnly);
        f.write(title.toUtf8());
        f.write(line);
    }
    f.flush();
    f.close();
}

PostBondDataAnalysis *BonderWorkHolder::getPostBondDataAnalysis() const
{
    return postBondDataAnalysis;
}

void BonderWorkHolder::correctPostBondShift(PrOffset &shift)
{
    shift.X += bwhConfig->postBondOffsetCorrectionX();
    shift.Y += bwhConfig->postBondOffsetCorrectionY();
}

void BonderWorkHolder::showPostBondShift(PrOffset shift)
{
    qCInfo(logCate()) << "Post bond shift:" << shift.toShortString();
    postBondDataAnalysis->appendData(shift);
}

TrayMap *BonderWorkHolder::getTrayMap() const
{
    return sensorTrayMap;
}

QPoint BonderWorkHolder::getLastSensorIndex() const
{
    return lastSensorIndex;
}

void BonderWorkHolder::setSensorState(QPoint index, SensorTrayMaterialData::MaterialState state)
{
    auto data = sensorTrayMap->getData<SensorTrayMaterialData>(index.x(), index.y());
    data->setMaterialState(state);
}

PrOffset BonderWorkHolder::getLastSensorOffset() const
{
    return lastSensorOffset;
}

BonderWorkHolder::BonderWorkHolder(TrayConfig *sensorTrayConfig,
                                   WorkHolderState *whState,
                                   SensorTrayConveyer *stcWorker)
    : Worker(WorkerName::BonderWorkHolder), sensorTrayConfig(sensorTrayConfig), whState(whState), stcWorker(stcWorker)
{
    sensorTrayMap = new TrayMap(sensorTrayConfig, "BonderWHSensorTrayMap",
                                QString("%1/BonderWHSensorTrayMap.json").arg(getWorkerConfigDir()),
                                &SensorTrayMaterialData::staticMetaObject);
    bwhConfig = new BonderWorkHolderConfig(this);
    bwhConfigFile = new ConfigFile("BonderWorkHolderConfig", bwhConfig,
                                   QString("%1/bonderWorkHolderConfig.json").arg(getWorkerConfigDir()));
    bwhConfigFile->populate();
    resetRowColumnIndex();
    lastSensorIndex.setX(-1);
    lastSensorIndex.setY(-1);
    postBondDataAnalysis = new PostBondDataAnalysis();
}

BonderWorkHolderConfig *BonderWorkHolder::getBwhConfig() const
{
    return bwhConfig;
}

double BonderWorkHolder::getBWHOpticYPrPos(int rowIndex)
{
    auto prLTPos = bwhOpticXY->getPos<XYModulePos>(XYPosName::PrLeftTop);
    int unitRowIndex = rowIndex / sensorTrayConfig->rowsInsideUnit();
    int rowIndexInsideUnit = rowIndex % sensorTrayConfig->rowsInsideUnit();
    double deltaY = unitRowIndex * sensorTrayConfig->unitRowDelta() + rowIndexInsideUnit * sensorTrayConfig->rowDelta();
    return prLTPos->yPos() - deltaY;
}

PrOffset BonderWorkHolder::getSensorPrOffset()
{
    PrOffset prOffset;
    if (!stSensorLocation->performPR(prOffset))
    {
        throw SilicolAbort(tr("Sensor pr failed!"));
    }
    return prOffset;
}

PrOffset BonderWorkHolder::getPostBondOffset()
{
    PrOffset prOffset;
    if (!postBondLocation->performPR(prOffset))
    {
        throw SilicolAbort(tr("Post bond pr failed!"));
    }
    return prOffset;
}

bool BonderWorkHolder::gotSensor() const
{
    return m_gotSensor;
}

void BonderWorkHolder::resetGotSensor()
{
    m_gotSensor = false;
}

void BonderWorkHolder::initEventHandle()
{
    bonderWHStopperCylLeft = MotionElementContainer::getIns()->getItem<SCCylinder>(CylName::BWHStopperLeft);
    bonderWHStopperCylRight = MotionElementContainer::getIns()->getItem<SCCylinder>(CylName::BWHStopperRight);
    bonderWHStageTraySensor = MotionElementContainer::getIns()->getItem<SCDI>(DiName::BWHStageTraySNR);
    bonderWHBeltZ = MotionElementContainer::getIns()->getItem<SingleAxis>(SAxisModuleName::BWHBeltZ);
    bonderWHTrackX = MotionElementContainer::getIns()->getItem<SingleAxis>(SAxisModuleName::BWHTrackX);
    bonderWHTrackZ = MotionElementContainer::getIns()->getItem<SingleAxis>(SAxisModuleName::BWHTrackZ);
    bonderWHBelt = MotionElementContainer::getIns()->getItem<SCAxis>(AxisName::BWHBelt);
    bonderWHVacuum = MotionElementContainer::getIns()->getItem<SCVacuum>(VacuumName::BWHVacuum);
    bwhOpticXY = MotionElementContainer::getIns()->getItem<XYModule>(XYModuleName::BWHOpticXY);
    bwhOpticZ = MotionElementContainer::getIns()->getItem<SingleAxis>(SAxisModuleName::BWHOpticZ);

    stSensorLocation = VisionManager::getIns().getVisionLocation(VLName::STSensorLocation);
    postBondLocation = VisionManager::getIns().getVisionLocation(VLName::PostBondLocation);

    beltRunDir = SCAxis::DirEnumInfo().nameToEnum(bwhConfig->beltRunDirection());
}

void BonderWorkHolder::startEventHandle(const WorkerEvent &event)
{
    Q_UNUSED(event);
    hasSynchronizeHasTray = false;
    hasMoveBeltToCheckHasTrayAtExitOfSTC = false;
    isDispWHStopped = false;
    resetRowColumnIndex();
    pushEvent(WorkerEvent("getNewTray"));
}

void BonderWorkHolder::allWorkerStopped()
{
    isDispWHStopped = false;
}

void BonderWorkHolder::purgeEventHandle()
{
    purgeEventHandled();
}

void BonderWorkHolder::resetEventHandle()
{
    isDispWHStopped = false;
}

void BonderWorkHolder::homeWorker(bool moveToReadyPosAfterHome)
{
    stcWorker->stcKicker->set(false, false);
    bonderWHStopperCylRight->set(false, false);
    stcWorker->stcKicker->wait(false);
    bonderWHStopperCylRight->wait(false);

    bwhOpticZ->axis()->home(false);
    bwhOpticXY->yAxis()->home(false);
    bonderWHTrackZ->axis()->home();
    bonderWHBeltZ->axis()->home();
    bonderWHTrackX->axis()->home();
    bwhOpticZ->axis()->waitHomeDone();
    bwhOpticXY->yAxis()->waitHomeDone();

    if (moveToReadyPosAfterHome)
    {
        bwhOpticZ->moveTo(SAxisPosName::BondPrPos, false);
        bwhOpticXY->yAxis()->absMove(getBWHOpticYPrPos(0), false);
        bonderWHTrackZ->moveTo(SAxisPosName::UpPos, false);
        bonderWHTrackX->moveTo(SAxisPosName::LoadTrayPos);
        bonderWHTrackZ->waitArrivedPos(SAxisPosName::UpPos);
        bonderWHBeltZ->moveTo(SAxisPosName::UpPos);
        bwhOpticZ->waitArrivedPos(SAxisPosName::BondPrPos);
        bwhOpticXY->yAxis()->waitArrivedPos();
    }
}
